//
// Created by wangjunjun on 2024/3/11.
//

#include "lcmcmd.h"
#include "../log/log.h"
#include "../config/configmanager.h"
#include "json.h"
#include "value.h"
std::shared_ptr<ClcmCmd> ClcmCmd::m_Instance= nullptr;
std::once_flag ClcmCmd::m_flag;

ClcmCmd::ClcmCmd():m_speed_x(0.0),
                   m_speed_y(0.0),
                   m_speed_w(0.0),
                   m_goal_callback_flag(-1),
                   m_dog_cur_status(0),
                   m_cout(0),
                   m_udpSocket(0),
                   m_port(1234){
    memset(m_v_des_lcm, 0 ,sizeof(m_v_des_lcm));
    memset(m_swing_height_lcm, 0 ,sizeof(m_swing_height_lcm));
    memset(m_offset_y_lcm, 0 ,sizeof(m_offset_y_lcm));
    memset(m_w_z, 0 ,sizeof(m_w_z));
    m_parameter_req = control_parameter_request_lcmt();
    memset(&m_serverAddr, 0, sizeof(m_serverAddr));
    memset(&m_clientAddr, 0, sizeof(m_clientAddr));
}
ClcmCmd::~ClcmCmd(){

}
void ClcmCmd::Start(){
    //读取配置文件
    std::shared_ptr<CConfigManager> pIntance =  CConfigManager::GetInstance();
    if(nullptr != pIntance){
        std::string  strPath = "./CFG/config";
        Json::Value tempValue = Json::Value::null;
        if(!pIntance->loadConfig(strPath, tempValue)){
            LOG_ERROR("Loading Config error!");
            return;
        } else{
            if(tempValue.isMember("Sig") && tempValue["Sig"].isObject()&&
               tempValue["Sig"].isMember("SigPort") &&  tempValue["Sig"]["SigPort"].isInt()){
                LOG_INFO("======== port {}", tempValue["Sig"]["SigPort"].asInt());
                m_port =  tempValue["Sig"]["SigPort"].asInt();
            }
        }
    }
    m_udpSocket = socket(AF_INET, SOCK_DGRAM, 0);
    if(m_udpSocket < 0 ){
        LOG_ERROR("Error Opening Socket!");
        return;
    }
    //将socket设置非阻塞模式
    int flags = fcntl(m_udpSocket, F_GETFL, 0);
    fcntl(m_udpSocket, F_SETFL, flags | O_NONBLOCK);


    m_serverAddr.sin_family = AF_INET;
    m_serverAddr.sin_addr.s_addr = INADDR_ANY;
    m_serverAddr.sin_port = htons(m_port);

    if (bind(m_udpSocket, (struct sockaddr *)&m_serverAddr, sizeof(m_serverAddr)) < 0) {
        LOG_ERROR("Bind failed");
        return ;
    }
    signal(SIGINT, signalHandler);
    //LCM init stage
    lcm::LCM lcm;

    if (!lcm.good())
        return ;
    m_parameter_req.requestNumber = 0;

    m_w_z[0]=0.0f;
    m_w_z[1]=0.0f;
    m_w_z[2]=0.0f;
    m_w_z[3]=0.0f;
    m_w_z[4]=0.0f;

    //set robot  for slam & nav
    m_v_des_lcm[0]=0.0;
    m_v_des_lcm[1]=0.0;
    m_v_des_lcm[2]=0.0;
    dance_v_des(lcm, m_parameter_req, m_v_des_lcm);

    m_swing_height_lcm[0]=.05f;
    m_swing_height_lcm[1]=.05f;
    m_swing_height_lcm[2]=.05f;
    m_swing_height_lcm[3]=.05f;
    dance_swing_height(lcm, m_parameter_req, m_swing_height_lcm);

    m_offset_y_lcm[0]=.035f;
    m_offset_y_lcm[1]=.035f;
    m_offset_y_lcm[2]=.035f;
    m_offset_y_lcm[3]=.035f;
    dance_offset_y(lcm, m_parameter_req, m_offset_y_lcm);

    dance_gait_number(lcm, m_parameter_req, 4);

    dance_start(lcm, m_parameter_req, 3);
    char data_buff[1024];
    socklen_t client_len = sizeof(m_clientAddr);
    long long last_control_timestamp;
    while (true) {
        // 获取当前时间戳
        auto current_time = std::chrono::system_clock::now();
        auto current_timestamp = std::chrono::time_point_cast<std::chrono::milliseconds>(current_time).time_since_epoch().count();

        fd_set readfds;
        FD_ZERO(&readfds);
        FD_SET(m_udpSocket, &readfds);

        struct timeval timeout;
        timeout.tv_sec = 1;  // 设置超时时间为1秒
        timeout.tv_usec = 0;

        int ready = select(m_udpSocket + 1, &readfds, NULL, NULL, &timeout);

        if (ready == -1) {;
            LOG_ERROR("select error");
            close(m_udpSocket);
            return ;
        } else if (ready == 0) {
            m_v_des_lcm[0]=0.;
            m_v_des_lcm[1]=0.;
            m_v_des_lcm[2]=0.;
            dance_v_des(lcm, m_parameter_req, m_v_des_lcm);
            LOG_INFO("In the previous execution,  no UDP data is received after 1 second.");
        } else {
            if (FD_ISSET(m_udpSocket, &readfds)) {

                ssize_t recv_len = recvfrom(m_udpSocket, data_buff, sizeof(data_buff), 0,
                                            (struct sockaddr *)&m_clientAddr, &client_len);
                if (recv_len < 0) {
                    LOG_ERROR("Error in recvfrom");
                    close(m_udpSocket);
                    return ;
                }


                // 解析接收到的数据
                std::string data_str(data_buff, recv_len);
                std::istringstream iss(data_str);
                double float1, float2;
                long long received_timestamp;
                std::string crc;
                iss >> float1 >> float2 >> received_timestamp >> crc;

                // 判断与接收到的时间戳的延迟
                if (current_timestamp - received_timestamp  < 1500) {

                    LOG_INFO("Delay is not greater than 1500ms");
                    // 执行CRC校验
                    std::string data_for_crc = data_str.substr(0, data_str.rfind(' '));  // 去除最后的CRC校验部分
                    std::string calculated_crc = calculateCRC32(data_for_crc);

                    if (calculated_crc == crc) {
                        LOG_INFO("CRC check passed");
                    } else {
                        LOG_ERROR("CRC check failed");
                    }
                    LOG_INFO("Received float 1: {} ", float1);
                    LOG_INFO("Received float 2: {}", float2);
                    LOG_INFO("Timestamp: {}", current_timestamp);
                    LOG_INFO("received_timestamp: {}", received_timestamp);
                    // 输出解析得到的数据
                    m_v_des_lcm[0]=float1;
                    m_v_des_lcm[1]=0.;
                    m_v_des_lcm[2]=float2;
                    dance_v_des(lcm, m_parameter_req, m_v_des_lcm);
                } else {
                    m_v_des_lcm[0]=0.;
                    m_v_des_lcm[1]=0.;
                    m_v_des_lcm[2]=0.;
                    dance_v_des(lcm, m_parameter_req, m_v_des_lcm);
                    LOG_INFO("Delay is greater than 1500ms" );
                }
            }
        }
    }

    close(m_udpSocket);
    return ;

}
std::shared_ptr<ClcmCmd> ClcmCmd::GetInStance() {
    std::call_once(m_flag, []() {
        m_Instance = std::shared_ptr<ClcmCmd>(new ClcmCmd(),
                                              [](ClcmCmd* ptr){delete ptr;});
    });
    return m_Instance;
}

void ClcmCmd::dance_start(lcm::LCM &lcm, control_parameter_request_lcmt &_parameter_request_lcmt, double value){

    int8_t parameterKind = 1;
    int8_t requestKind = 3;

    strcpy((char *)_parameter_request_lcmt.name, (char *)"dance_start");
    memcpy(_parameter_request_lcmt.value, &value, sizeof(value));

    _parameter_request_lcmt.parameterKind = parameterKind;
    _parameter_request_lcmt.requestKind = requestKind;
    _parameter_request_lcmt.requestNumber++;

    lcm.publish("interface_request", &_parameter_request_lcmt);
}

void ClcmCmd::dance_gait_number(lcm::LCM &lcm, control_parameter_request_lcmt &_parameter_request_lcmt, double value){
    int8_t parameterKind = 1;
    int8_t requestKind = 3;

    strcpy((char *)_parameter_request_lcmt.name, (char *)"dance_gait_number");
    memcpy(_parameter_request_lcmt.value, &value, sizeof(value));

    _parameter_request_lcmt.parameterKind = parameterKind;
    _parameter_request_lcmt.requestKind = requestKind;
    _parameter_request_lcmt.requestNumber++;

    lcm.publish("interface_request", &_parameter_request_lcmt);
}

//Vec4f
void ClcmCmd::dance_offset_y(lcm::LCM &lcm, control_parameter_request_lcmt &_parameter_request_lcmt, float * value){
    int8_t parameterKind = 5;
    int8_t requestKind = 3;

    strcpy((char *)_parameter_request_lcmt.name, (char *)"dance_offset_y");
    memcpy(_parameter_request_lcmt.value, value, sizeof(float)*4);

    _parameter_request_lcmt.parameterKind = parameterKind;
    _parameter_request_lcmt.requestKind = requestKind;
    _parameter_request_lcmt.requestNumber++;

    lcm.publish("interface_request", &_parameter_request_lcmt);
}
//Vec4f
void ClcmCmd::dance_swing_height(lcm::LCM &lcm, control_parameter_request_lcmt &_parameter_request_lcmt, float * value){
    int8_t parameterKind = 5;
    int8_t requestKind = 3;

    strcpy((char *)_parameter_request_lcmt.name, (char *)"dance_swing_height");
    memcpy(_parameter_request_lcmt.value, value, sizeof(float)*4);

    _parameter_request_lcmt.parameterKind = parameterKind;
    _parameter_request_lcmt.requestKind = requestKind;
    _parameter_request_lcmt.requestNumber++;

    lcm.publish("interface_request", &_parameter_request_lcmt);
}
//Vec3D
void ClcmCmd::dance_v_des(lcm::LCM &lcm, control_parameter_request_lcmt &_parameter_request_lcmt, double * value){
    int8_t parameterKind = 3;
    int8_t requestKind = 3;

    strcpy((char *)_parameter_request_lcmt.name, (char *)"dance_v_des");
    memcpy(_parameter_request_lcmt.value, value, sizeof(double)*3);

    _parameter_request_lcmt.parameterKind = parameterKind;
    _parameter_request_lcmt.requestKind = requestKind;
    _parameter_request_lcmt.requestNumber++;

    lcm.publish("interface_request", &_parameter_request_lcmt);
    LOG_INFO("dance_v_des: {}", _parameter_request_lcmt.requestNumber );
}

void ClcmCmd::signalHandler(int signum){
    // action::action_t act;
    if(m_Instance!= nullptr){
        m_Instance->sigHandler(signum);
    }

}
void  ClcmCmd::sigHandler(int signum){
    lcm::LCM lcm;
    if (!lcm.good())
        ClcmCmd::m_v_des_lcm[0]=0.0;
    ClcmCmd::m_v_des_lcm[1]=0.0;
    ClcmCmd::m_v_des_lcm[2]=0.0;
    ClcmCmd::dance_v_des(lcm, ClcmCmd::m_parameter_req, ClcmCmd::m_v_des_lcm);
    ClcmCmd::dance_gait_number(lcm, ClcmCmd::m_parameter_req, 4);
    ClcmCmd::dance_start(lcm, ClcmCmd::m_parameter_req, 0);

}

std::string  ClcmCmd::calculateCRC32(const std::string &data){
    unsigned char result[MD5_DIGEST_LENGTH];
    MD5(reinterpret_cast<const unsigned char *>(data.c_str()), data.length(), result);

    // 转换结果为十六进制字符串
    std::stringstream crc_stream;
    for (int i = 0; i < MD5_DIGEST_LENGTH; ++i) {
        crc_stream << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(result[i]);
    }

    return crc_stream.str();
}